/******************** (C) COPYRIGHT 2013 InMotion ******************************
*File Name   : Median_Filter.c
*Author      : 郭盖华               
*Version     : V0.0.1
*Date        : 2013-5-1
*Description : 中值滤波文件
*******************************************************************************/

/* Includes ------------------------------------------------------------------*/
#include "median_filter.h"
#include "my_type.h"

/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Variables -----------------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function ------------------------------------------------------------------*/

/*******************************************************************************
* Function Name  : Median_Filter_int_Clear
* Description    : 整型数据中值滤波清零
* Input          : filter 滤波器指针
* Output         : None
* Notes          : None
*******************************************************************************/
void Median_Filter_int_Clear(MEDIAN_FILTER_Int_DEF *filter)
{
	u32 i;
	for(i = 0 ; i < filter->tap ; i ++)
		filter->delay[i] = 0;	
}

/*******************************************************************************
* Function Name  : Median_Filter_int
* Description    : 整型数据中值滤波
* Input          : filter 滤波器指针，invar输入值
* Output         : 滤波输出值
* Notes          : None
*******************************************************************************/
s32 Median_Filter_int(MEDIAN_FILTER_Int_DEF *filter, s32 invar) 
{
	s32 i, j, *pdata;
	s64 sum = 0;
	s32 max1, max2, min1, min2;

	//前移一位
	for(i = 0 ; i < filter->tap - 1 ; i ++)
	{
		filter->delay[i] = filter->delay[i + 1];
		pdata ++;
		sum += filter->delay[i];
	}
	filter->delay[i] = invar;
	sum += filter->delay[i];

	//最大值
	max1 = filter->delay[0];
	for(i = 0 ; i < filter->tap ; i ++)
	{
		if(max1 < filter->delay[i])
		{
			max1 = filter->delay[i];
			j = i;
		}
	}

	//第二大值
	if(j == 0)
		max2 = filter->delay[1];
	else
		max2 = filter->delay[0];

	for(i = 0 ; i < filter->tap ; i ++)
	{
		if((max2 < filter->delay[i]) && (i != j))
			max2 = filter->delay[i];			
	}

	//最小值
    min1 = filter->delay[0];
	for(i = 0 ; i < filter->tap ; i ++)
	{
		if(min1 > filter->delay[i])
		{
			min1 = filter->delay[i];
			j = i;
		}
	}

	//第二小值
	if(j == 0)
		min2 = filter->delay[1];
	else
		min2 = filter->delay[0];

	for(i = 0 ; i < filter->tap ; i ++)
	{
		if((min2 > filter->delay[i]) && (i != j))
			min2 = filter->delay[i];			
	}

	//去掉最大两个, 与最小两个	剩下的求平均
	sum -= max1;
	sum -= max2;
	sum -= min1;
	sum -= min2;
	
	invar = sum / (filter->tap - 4);

	return invar;
}

/********************** (C) COPYRIGHT 2013 InMotion *****END OF FILE************/ 
